#ifndef MAT_22_H
#define MAT_22_H

#include "vector.hpp"

namespace fixedpoint
{
    //2x3 matrix
    //Mostly a good space-saver instead of a common 3x3 matrix
    struct mat23; //Forward declaration

    //2x2 fixed-point matrix
    struct mat22
    {
public:
        //Column-major ordering
        fixpoint _11, _21, _12, _22;
        //Constructors default to unit matrix
        mat22(): _11(1), _21(0), _12(0), _22(1) {}
        //All initializers are in column-major order
        mat22(fixpoint a, fixpoint b,
              fixpoint c, fixpoint d): _11(a), _21(b), _12(c), _22(d) {}

        //Array initializers
        mat22(const fixpoint a[4]): _11(a[0]), _21(a[1]), _12(a[2]), _22(a[3]) {}
        //Column vector initializers
        mat22(const vec2d& a, const vec2d& b): _11(a.x), _21(a.y),
                _12(b.x), _22(b.y) {}

        //Assignment operators
        mat22& operator =(const fixpoint a[4])
        {
            _11 = a[0];
            _21 = a[1];
            _12 = a[2];
            _22 = a[3];
            return *this;
        }

        //Modifying assignment
        mat22& operator +=(const mat22& a)
        {
            _11 += a._11;
            _21 += a._21;
            _12 += a._12;
            _22 += a._22;
            return *this;
        }
        mat22& operator -=(const mat22& a)
        {
            _11 -= a._11;
            _21 -= a._21;
            _12 -= a._12;
            _22 -= a._22;
            return *this;
        }
        mat22& operator *=(const mat22& a)
        {
            mat22 temp = trans();
            _11 = temp[0].dot(a[0]);
            _21 = temp[1].dot(a[0]);
            _12 = temp[0].dot(a[1]);
            _22 = temp[1].dot(a[1]);
            return *this;
        }
        //Scalar modifying multiplication
        mat22& operator *=(const fixpoint a)
        {
            _11 *= a;
            _21 *= a;
            _12 *= a;
            _22 *= a;
            return *this;
        }
        mat22& operator /=(const fixpoint a)
        {
            fixpoint r = fixpoint(fixpoint::RAW, UNIT) / a;
            _11 *= r;
            _21 *= r;
            _12 *= r;
            _22 *= r;
            return *this;
        }

        //Vector column accessors
        //Watch out, someone can use this to access arbitrary memory!
        const vec2d& operator [](unsigned i) const
        {
            return *(reinterpret_cast<const vec2d*>(&_11) + i);
        }
        vec2d& operator [](unsigned i)
        {
            return *(reinterpret_cast<vec2d*>(&_11) + i);
        }

        //Individual member access
        //Rows and columns start at 1
        const fixpoint& operator ()(unsigned r, unsigned c) const
        {
            return *(reinterpret_cast<const fixpoint*>(&_11) + (r-1)+2*(c-1));
        }
        fixpoint& operator ()(unsigned r, unsigned c)
        {
            return *(reinterpret_cast<fixpoint*>(&_11) + (r-1)+2*(c-1));
        }

        //Basic binary operators
        mat22 operator *(const mat22& a) const
        {
            return mat22(_11*a._11 + _12*a._21,
                         _21*a._11 + _22*a._21,
                         _11*a._12 + _12*a._22,
                         _21*a._12 + _22*a._22);
        }
        mat23 operator *(const mat23& a) const; //Because of circular dependencies, this must be defined and linked in a source file

        mat22 operator +(const mat22& a) const
        {
            return mat22(_11 + a._11, _21 + a._21,
                         _12 + a._12, _22 + a._22);
        }
        mat22 operator -(const mat22& a) const
        {
            return mat22(_11 - a._11, _21 - a._21,
                         _12 - a._12, _22 - a._22);
        }
        vec2d operator *(const vec2d& a) const
        {
            return vec2d(_11*a.x + _12*a.y,
                         _21*a.x + _22*a.y);
        }
        //Scalar operators
        mat22 operator *(const fixpoint a) const
        {
            return mat22(_11*a, _21*a,
                         _12*a, _22*a);
        }
        mat22 operator /(const fixpoint a) const
        {
            fixpoint r = fixpoint(fixpoint::RAW, UNIT) / a;
            return mat22(_11*r, _21*r,
                         _12*r, _22*r);
        }

        //Geometric functions
        //Transverse matrix
        mat22 trans() const
        {
            return mat22(_11, _12,
                         _21, _22);
        }

        //Determinant
        fixpoint det() const
        {
            return fixpoint(_11*_22 - _21*_12);
        }

        //Inverse matrix
        //(2x2 case is the only really simple inverse case)
        mat22 inv() const
        {
            fixpoint recip = fixpoint(UNIT)/det();
            return mat22(_22, -_21,
                         -_12, _11) * recip;
        }
    };

}

#endif
